{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# SmartProjectionPoseFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`SmartProjectionPoseFactor<CALIBRATION>` is a \"smart\" factor optimized for visual SLAM problems where the **camera calibration is known and fixed**, and only the camera **poses** are being optimized.\n",
        "It inherits from `SmartProjectionFactor` but simplifies the setup by taking a single shared `CALIBRATION` object (e.g., `Cal3_S2`, `Cal3Bundler`) assumed to be used by all cameras observing the landmark.\n",
        "\n",
        "Key characteristics:\n",
        "- **Implicit Point:** Like its base class, the 3D point is not an explicit variable.\n",
        "- **Fixed Calibration:** Assumes a single, known calibration `K` for all measurements.\n",
        "- **Pose Variables:** The factor connects `Pose3` variables directly.\n",
        "- **`Values` Requirement:** Requires `Pose3` objects in the `Values` container for its keys.\n",
        "- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n",
        "\n",
        "**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n",
        "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`.\n",
        "\n",
        "If you are using the factor, please cite:\n",
        "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import (\n",
        "    Values,\n",
        "    Point2,\n",
        "    Point3,\n",
        "    Pose3,\n",
        "    Rot3,\n",
        "    NonlinearFactorGraph,\n",
        "    SmartProjectionParams,\n",
        "    SmartProjectionPoseFactorCal3_S2,\n",
        "    Cal3_S2,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X  # Key for Pose3 variable"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating and Adding Measurements"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "1. Create the factor with a noise model, the shared calibration `K`, and parameters.\n",
        "2. Add measurements (2D points) and the corresponding pose keys."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Smart factor involves 3 measurements.\n",
            "SmartFactorPose: SmartProjectionPoseFactor, z = \n",
            " SmartProjectionFactor\n",
            "linearizationMode: 0\n",
            "triangulationParameters:\n",
            "rankTolerance = 1\n",
            "enableEPI = 0\n",
            "landmarkDistanceThreshold = -1\n",
            "dynamicOutlierRejectionThreshold = -1\n",
            "useLOST = 0\n",
            "noise model\n",
            "\n",
            "result:\n",
            "no point, status = 1\n",
            "\n",
            "SmartFactorBase, z = \n",
            "measurement 0, px = \n",
            "150\n",
            "505\n",
            "noise model = unit (2) \n",
            "measurement 1, px = \n",
            "470\n",
            "495\n",
            "noise model = unit (2) \n",
            "measurement 2, px = \n",
            "480\n",
            "150\n",
            "noise model = unit (2) \n",
            "  keys = { x0 x1 x2 }\n"
          ]
        }
      ],
      "source": [
        "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "smart_params = SmartProjectionParams() # Use default params\n",
        "K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
        "\n",
        "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n",
        "\n",
        "# Add measurements and keys (Pose keys)\n",
        "smart_factor.add(Point2(150, 505), X(0))\n",
        "smart_factor.add(Point2(470, 495), X(1))\n",
        "smart_factor.add(Point2(480, 150), X(2))\n",
        "\n",
        "print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n",
        "smart_factor.print(\"SmartFactorPose: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The `.error(values)` method uses the `Pose3` objects from `values` and the fixed `K` to triangulate the point and compute the error."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Triangulated point result: Status.VALID\n",
            "Triangulated point: [0.28416823 1.95555615 5.67688675]\n",
            "\n",
            "Total reprojection error (0.5 * sum(err^2/sigma^2)): 120243.1626\n"
          ]
        }
      ],
      "source": [
        "# Create Values containing Pose3 objects\n",
        "values = Values()\n",
        "pose0 = Pose3(Rot3.Ypr(0.1, 0, 0), Point3(-1, 0, 0.5))\n",
        "pose1 = Pose3(Rot3.Ypr(0.0, -0.1, 0), Point3(1, 0, 0.5))\n",
        "pose2 = Pose3(Rot3.Ypr(0, 0.0, 0.2), Point3(0, 1, 0.5))\n",
        "\n",
        "values.insert(X(0), pose0)\n",
        "values.insert(X(1), pose1)\n",
        "values.insert(X(2), pose2)\n",
        "\n",
        "# Triangulate first to see the implicit point\n",
        "point_result = smart_factor.point(values)\n",
        "print(f\"Triangulated point result: {point_result.status}\")\n",
        "\n",
        "if point_result.valid():\n",
        "   print(f\"Triangulated point: {point_result.get()}\")\n",
        "   # Calculate error\n",
        "   total_error = smart_factor.error(values)\n",
        "   print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
        "else:\n",
        "   print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "linearize_header_md"
      },
      "source": [
        "## Linearization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "linearize_desc_md"
      },
      "source": [
        "Linearization works similarly to `SmartProjectionFactor`, producing a linear factor (default: Hessian) connecting the `Pose3` variables."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "linearize_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Linearized Factor (showing HessianFactor structure):\n",
            "\n",
            " keys: x0(6) x1(6) x2(6) \n",
            "Augmented information matrix: [\n",
            "\t202102, 6373.47, -59741.9, 4386.91, -35145.2, 11091.1, -115337, -44197.1, 12233.6, -8181.39, 19268.4, -7521.18, -81683.2, -724.877, 5044.41, -484.005, 14404.6, -5767.48, 89097.4;\n",
            "\t6373.47, 81115.2, -30292.7, 14717.5, -2404.93, -3343.21, 63410.4, 20584.6, -5335.01, 3787.66, -10602.2, 4111.71, -60743.7, -88128.4, 38992.5, -18242.3, 11072.3, -3393.47, 105458;\n",
            "\t-59741.9, -30292.7, 27634, -6415.45, 10844.3, -1981.98, 10553.8, 5348.27, -1607.72, 998.024, -1760.06, 696.401, 44568.2, 31147.8, -15125.4, 6542.51, -7986, 2832.46, -62376.9;\n",
            "\t4386.91, 14717.5, -6415.45, 2722.09, -996.496, -424.653, 9577.1, 3000.7, -765.229, 551.36, -1601.54, 620.327, -12253.7, -15890.6, 7106.5, -3294.65, 2225.84, -703.855, 20429.2;\n",
            "\t-35145.2, -2404.93, 10844.3, -996.496, 6132.48, -1869.56, 18982.5, 7333.62, -2035.73, 1357.9, -3171.11, 1238.23, 15136.7, 1537.88, -1499.49, 376.242, -2675.09, 1054.42, -17138.8;\n",
            "\t11091.1, -3343.21, -1981.98, -424.653, -1869.56, 777.22, -9389.67, -3428.6, 932.466, -633.633, 1569.05, -611.241, -1827.86, 3981.42, -1495.55, 805.333, 305.895, -169.934, 204.626;\n",
            "\t-115337, 63410.4, 10553.8, 9577.1, 18982.5, -9389.67, 210063, -32012.2, 19847.7, -6615.86, -35357.8, 12992.1, -83063.6, 2877.07, 3675.5, 245.696, 14633.2, -5901.9, 91375.5;\n",
            "\t-44197.1, 20584.6, 5348.27, 3000.7, 7333.62, -3428.6, -32012.2, 79820.6, -31086.7, 15340.5, 5564.44, -1509.55, 71381, -87978.3, 31244.3, -17668.1, -12223.3, 5946.04, -40235;\n",
            "\t12233.6, -5335.01, -1607.72, -765.229, -2035.73, 932.466, 19847.7, -31086.7, 12383.1, -5991.81, -3406.72, 1051.42, -29836.9, 33051, -11561.9, 6625.01, 5124.61, -2447.34, 18485.3;\n",
            "\t-8181.39, 3787.66, 998.024, 551.36, 1357.9, -633.633, -6615.86, 15340.5, -5991.81, 2949.34, 1147.27, -319.227, 13846.5, -16832.1, 5966.68, -3379.52, -2372.04, 1151.02, -7909.48;\n",
            "\t19268.4, -10602.2, -1760.06, -1601.54, -3171.11, 1569.05, -35357.8, 5564.44, -3406.72, 1147.27, 5951.86, -2185.74, 14119.3, -690.008, -543.913, -82.7916, -2486.55, 1005.27, -15442.2;\n",
            "\t-7521.18, 4111.71, 696.401, 620.327, 1238.23, -611.241, 12992.1, -1509.55, 1051.42, -319.227, -2185.74, 806.502, -4768.82, -371.498, 426.945, -95.4627, 842.323, -333.351, 5486.34;\n",
            "\t-81683.2, -60743.7, 44568.2, -12253.7, 15136.7, -1827.86, -83063.6, 71381, -29836.9, 13846.5, 14119.3, -4768.82, 149690, -5708.64, -6412.92, -549.716, -26368.6, 10641.3, -162324;\n",
            "\t-724.877, -88128.4, 31147.8, -15890.6, 1537.88, 3981.42, 2877.07, -87978.3, 33051, -16832.1, -690.008, -371.498, -5708.64, 160163, -64108.9, 32675.7, 347.645, -2041.12, -63443.5;\n",
            "\t5044.41, 38992.5, -15125.4, 7106.5, -1499.49, -1495.55, 3675.5, 31244.3, -11561.9, 5966.68, -543.913, 426.945, -6412.92, -64108.9, 26167.1, -13114.9, 1394.39, 202.071, 34971.1;\n",
            "\t-484.005, -18242.3, 6542.51, -3294.65, 376.242, 805.333, 245.696, -17668.1, 6625.01, -3379.52, -82.7916, -95.4627, -549.716, 32675.7, -13114.9, 6668.86, -37.4944, -372.943, -13620.5;\n",
            "\t14404.6, 11072.3, -7986, 2225.84, -2675.09, 305.895, 14633.2, -12223.3, 5124.61, -2372.04, -2486.55, 842.323, -26368.6, 347.645, 1394.39, -37.4944, 4647.64, -1867.77, 28880.4;\n",
            "\t-5767.48, -3393.47, 2832.46, -703.855, 1054.42, -169.934, -5901.9, 5946.04, -2447.34, 1151.02, 1005.27, -333.351, 10641.3, -2041.12, 202.071, -372.943, -1867.77, 773.191, -10827.4;\n",
            "\t89097.4, 105458, -62376.9, 20429.2, -17138.8, 204.626, 91375.5, -40235, 18485.3, -7909.48, -15442.2, 5486.34, -162324, -63443.5, 34971.1, -13620.5, 28880.4, -10827.4, 240486\n",
            "]\n"
          ]
        }
      ],
      "source": [
        "graph = NonlinearFactorGraph()\n",
        "graph.add(smart_factor)\n",
        "\n",
        "# Linearize (default mode is HESSIAN)\n",
        "linear_factor = smart_factor.linearize(values)\n",
        "\n",
        "if linear_factor:\n",
        "    print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n",
        "    # Cast to HessianFactor to print its details\n",
        "    hessian_factor = gtsam.HessianFactor(linear_factor)\n",
        "    if hessian_factor:\n",
        "         hessian_factor.print()\n",
        "    else:\n",
        "         print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n",
        "else:\n",
        "    print(\"Linearization failed (likely due to triangulation degeneracy)\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
